#include <fcntl.h>

#include <sys/types.h>
#include <sys/stat.h>
#include <sys/mman.h>
#include <sys/time.h>
#include <dirent.h>
#include <stdlib.h>
#include <stdio.h>
#include <signal.h>
#include "objectarray.h"

#if 1

#include "lane.h"
//#include "uvccamera-lib.h"


static LANE_POSITION_DATA lanePosData ;
static LDWS_CTRL ldwsCtrl ;

int gW , gH , gRW , gRH;
int initlanePosData(void){
	memset(&lanePosData , 0 , sizeof(lanePosData)) ;
	memset(&ldwsCtrl , 0 , sizeof(ldwsCtrl)) ;
//	memset(&thresholdCtrl , 0 , sizeof(thresholdCtrl)) ;

	gW = 116 ;
	gH = 100 ;
	gRW = 100 ;
	gRH = 100 ;

	lanePosData.startXL = 510 ; //490;//500 ; // 465
	lanePosData.startYL = 390 ;
	lanePosData.wL = gW-10 ; //gW-20 ; // 115
	lanePosData.hL = gH ;
	lanePosData.slopeL = 1.8 ;

	//lanePosData.startXR = (lanePosData.startXL + gW + (LANE_GAP_W*2)) ;//671;//666 ; //665
	lanePosData.startXR = (lanePosData.startXL + gW + (LANE_GAP_W*2) + gRW) ;//671;//666 ; //665
	lanePosData.startYR = lanePosData.startYL + gRH ; //390 ; // 390
	lanePosData.wR = gW ; // 115
	lanePosData.hR = gH ; // 100
	lanePosData.slopeR = 2 ; //1.7 ;//1.8 ;
	
	lanePosData.thresholdL = 120 ;
	lanePosData.thresholdR = lanePosData.thresholdL ;
	lanePosData.thresholdC = lanePosData.thresholdL ;
	
	lanePosData.thresholdU = 120 ;
	lanePosData.thresholdV = 120 ;

	lanePosData.startXC = lanePosData.startXL+lanePosData.wL ; //+ (gW/2) ; 
	lanePosData.startYC = lanePosData.startYL + gH/4 ; // + 20 ; 
	lanePosData.wC = 70 ;//LANE_GAP_W*2 ; //(gW/2) ;
	lanePosData.hC = (LANE_GAP_W*3) ;

	ldwsCtrl.threshold = 20 ; //100 ;
	
	return 0 ;
}

int setlanePosData(u8 isRight , u32 startX , u32 startY , float slope , u32 w , u32 h , u8 threshold , u8 thresholdU , u8 thresholdV){ 
	if(isRight == 1){
		lanePosData.startXR = startX ;
		lanePosData.startYR = startY ;
		
		lanePosData.wR = w ;
		lanePosData.hR = h ;
		
		//lanePosData.slopeR = slope ;
	}else{
		lanePosData.startXL = startX ;
		lanePosData.startYL = startY ;

		lanePosData.wL = w ;
		lanePosData.hL = h ;

		//lanePosData.slopeL = slope ;
	}

	lanePosData.thresholdL = threshold ;
	lanePosData.thresholdU = thresholdU ;
	lanePosData.thresholdV = thresholdV ;
	
	return 0 ;
}


int getlanePosData(u8 isRight , u32* startX , u32* startY , float* slope , u32* w , u32* h,u8* threshold,u8* thresholdU,u8* thresholdV){	
	if(isRight == 1){
		*(u32*)startX = lanePosData.startXR ;
		*(u32*)startY = lanePosData.startYR ;
		
		*(u32*)w = lanePosData.wR ;
		*(u32*)h = lanePosData.hR ;
		
		*(float*)slope = lanePosData.slopeR ;
	}else{
		*(u32*)startX = lanePosData.startXL ;
		*(u32*)startY = lanePosData.startYL ;

		*(u32*)w = lanePosData.wL ;
		*(u32*)h = lanePosData.hL ;

		*(float*)slope = lanePosData.slopeL ;
	}

	*(u8*)threshold = lanePosData.thresholdL ;
	*(u8*)thresholdU = lanePosData.thresholdU ;
	*(u8*)thresholdV = lanePosData.thresholdV ;
	
	return 0 ;
}


int setlanePos(u32 startX , u32 startY ){ 

	if(startX<(MJ_WIDTH-lanePosData.wR) && startX>lanePosData.wL && \
		startY>0 && startY<(MJ_HEIGHT-lanePosData.hR-LANE_GAP_H)){
#if 0
		lanePosData.startXR = (startX+LANE_GAP_W + gRW) ;
		lanePosData.startYR = startY+LANE_GAP_H + gRH ;//startY+LANE_GAP_H ;
#else
		lanePosData.startXR = (startX+ (lanePosData.wC/2) + 25) ; //(startX+LANE_GAP_W+40 ) ;
		lanePosData.startYR = startY+LANE_GAP_H+30  ;//startY+LANE_GAP_H ;
#endif

		lanePosData.startXL = (startX-20-116) ; //(startX-LANE_GAP_W-gW) ;
		lanePosData.startYL = startY+LANE_GAP_H ;

		// lanePosData.startXC = lanePosData.startXL ; //
		lanePosData.startXC = startX-(lanePosData.wC/2) ; // lanePosData.startXL+lanePosData.wL ;
		lanePosData.startYC = lanePosData.startYL+30 ; //lanePosData.startYL + gH/4 ;

	}
	
	return 0 ;
}

int getlanePos(u32* startX , u32* startY ){ 

	*(u32*)startX = (lanePosData.startXL + gW + LANE_GAP_W) ;
	*(u32*)startY = (lanePosData.startYL - LANE_GAP_H) ;
	
	return 0 ;
}



/*

R = Y + 1.402 (Cr-128)
G = Y - 0.34414 (Cb-128) - 0.71414 (Cr-128)
B = Y + 1.772 (Cb-128)

Y =      0.299  R + 0.587   G + 0.114 B
U =  - 0.1687  R - 0.3313 G +     0.5 B + 128
V =           0.5 R - 0.4187 G - 0.0813 B + 128


YCbCr[ Y - U:blue - V:red ]

*/

void get123Val(u8* addr , u32 X , u32 Y, u8* y , u8* u , u8* v){
	float tmpY , tmpU , tmpV ; 
	tmpY = *((u8*)addr + MJ_WIDTH*Y + X ) ;
	tmpU = 0 ;//*((u8*)addr + (MJ_WIDTH*MJ_HEIGHT) + (MJ_WIDTH/2)*(Y/2) + (X/2) ) ; // Cb
	tmpV = 0 ;//*((u8*)addr + (MJ_WIDTH*MJ_HEIGHT*5/4) + (MJ_WIDTH/2)*(Y/2) + (X/2)) ; // Cr

#if 1  // yuv
	*(u8*)y = tmpY ;
	*(u8*)u = tmpU ;
	*(u8*)v = tmpV ;
#else  // rgb
	*(u8*)y = (u8)(tmpY + 1.4*(tmpV-128)) ;
	*(u8*)u = (u8)(tmpY - 0.34*(tmpU-128) - 0.71*(tmpV-128)) ;
	*(u8*)v = (u8)(tmpY + 1.77*(tmpU-128)) ;
#endif	
}

void set123Val(u8* addr , u32 X , u32 Y , u8 y , u8 u , u8 v){

	//debugl("yuv x:[%d] - y:[%d]" , X , Y) ;
	if(y > 0 ){
		*((u8*)addr + MJ_WIDTH*Y + X ) = y ;
	}
	if(u > 0){
		*((u8*)addr + (MJ_WIDTH*MJ_HEIGHT) + (MJ_WIDTH/2)*(Y/2) + (X/2) ) = u ;
	}
	if(v > 0){
		//*((u8*)addr + MJ_WIDTH*MJ_HEIGHT*5/4 + (MJ_WIDTH*Y)/4 + X/2) = v ;
		*((u8*)addr + (MJ_WIDTH*MJ_HEIGHT*5/4) + (MJ_WIDTH/2)*(Y/2) + (X/2) ) = v ;
	}
}

static int tmpSumL=0 , tmpSumR=0 , tmpMaxL=0 , tmpMaxR=0 , tmpAvrL=0 , tmpAvrR=0  , tmpIndexL = 0  , tmpIndexR;
int getTargetAreaAverData(u8 isRight , u8* dataAddr , u32* sensitiveDataCnt){
	double tmpDataY=0 , tmpDataU=0 , tmpDataV=0 ;
	u32 tmpW = 0 , tmpH= 0 ;
	u32 tmp_x = 0 , tmp_y = 0 ;
	u32 tmp_X = 0 , tmp_Y = 0 ;
	float i , j , tmpSlope = 0 ; 
	u8 tmpY , tmpU , tmpV  ;
	u8 tmpThreshold = lanePosData.thresholdL ;
	u8 tmpThresholdU = lanePosData.thresholdU ;
	u8 tmpThresholdV = lanePosData.thresholdV ;

	u32 tmpSensitiveDataCnt=0 ;

	if(isRight == 1){
		tmpMaxR = 0 ;
		tmp_x = lanePosData.startXR ;
		tmp_y = lanePosData.startYR ;
		tmpW = lanePosData.wR ;
		tmpH = lanePosData.hR ;
		tmpSlope = lanePosData.slopeR ;

		tmpThreshold = lanePosData.thresholdR ;
	}else if(isRight == 0){
		tmpMaxL = 0 ;
		tmp_x = lanePosData.startXL ;
		tmp_y = lanePosData.startYL ;
		tmpW = lanePosData.wL ;
		tmpH = lanePosData.hL ;
		tmpSlope = lanePosData.slopeL ;

		tmpThreshold = lanePosData.thresholdL ;
	}else if(isRight == 2){
		u8 tmpValC = 0 ;
		tmp_x = lanePosData.startXC ;
		tmp_y = lanePosData.startYC ;
		tmpW = lanePosData.wC ;
		tmpH = lanePosData.hC ;
		tmpSlope = 0 ;

		//tmpValC = (lanePosData.thresholdR+lanePosData.thresholdL)/2 ;
		tmpValC = (lanePosData.thresholdR+lanePosData.thresholdL)/2 ;
#if 0
		if((lanePosData.thresholdR>180) || (lanePosData.thresholdL>180)){
			tmpThreshold = tmpValC+20 ;	
		}else{
			tmpThreshold = tmpValC ;
		}
#endif
	}

	

	for(i=0 ; i<tmpH ; i++ ){
		for(j=0 ; j<tmpW ; j++){ //YCbCr[ Y - U:blue - V:red ]
			if(isRight == 1)
				tmp_X = tmp_x+j+(u32)(i*tmpSlope) ;
			else
				tmp_X = tmp_x+j-(u32)(i*tmpSlope) ;
			
			tmp_Y = tmp_y+i ;


#if 1
			//if(i==0 ){
//			if(i==0 || j==0 || i==(tmpH-1) || j==(tmpW-1)){
			if( j==0 || i==(tmpH-1) || j==(tmpW-1)){
				// release must be delete
			//	set123Val((u8*)dataAddr , tmp_X , tmp_Y , 255 , 255 , 255) ;
			}else{
				get123Val((u8*)dataAddr , tmp_X , tmp_Y , &tmpY , &tmpU , &tmpV) ;
				tmpDataY += tmpY ; 
				tmpDataU += tmpU ;
				tmpDataV += tmpV ;

				if((tmpY > tmpMaxL) && (isRight == 0)){
					tmpMaxL = tmpY ;
				}
				
				if((tmpY > tmpMaxR) && (isRight == 1)){
					tmpMaxR = tmpY ;
				}

				if(isRight == 1){
					tmpIndexR++ ;
					tmpAvrR += tmpY ;
				}else if(isRight == 0){
					tmpAvrL += tmpY ;
					tmpIndexL++ ;
				}
				
			}

			//if((tmpY>=tmpThreshold) && (tmpU>=tmpThreshold) && (tmpV>=tmpThreshold) )
			//if((tmpY>=tmpThreshold) && (tmpU>=tmpThreshold) )
//			if(tmpY>=tmpThreshold && tmpU>= tmpThresholdU && tmpV>= tmpThresholdV)
			if(tmpY>=tmpThreshold)
			{
				// release must be delete
			//	set123Val((u8*)dataAddr , tmp_X , tmp_Y , 255 , 1 , 100) ;
				tmpSensitiveDataCnt++ ;
			}

#else
			{
				get123Val((u8*)dataAddr , tmp_X , tmp_Y , &tmpY , &tmpU , &tmpV) ;
				tmpDataY += tmpY ; 
				tmpDataU += tmpU ;
				tmpDataV += tmpV ;
			}

			if((tmpY>=180) && (tmpU>=180) && (tmpV>=180) ){
				set123Val((u8*)dataAddr , tmp_X , tmp_Y , 255 , 255 , 255) ;
			}
#endif
		}					
	}



	*(u32*)sensitiveDataCnt = tmpSensitiveDataCnt ;



	//debugl("yuv Y:[%02f] - U:[%02f] - V:[%02f]" , (u32)(dataY/(w*h)) , (u32)(dataU/(w*h)) , (u32)(dataV/(w*h))) ;
	//debugl("yuv %s Y:[%d] - U:[%d] - V:[%d]" , isRight?"R":"L", tmpY , tmpU , tmpV) ;

	//debugl("count {%s} - [%d]" , isRight?"R":"L", tmpCount) ;

    return 0 ;
}


int getSysTimeMs(u8 isMs){
	struct timeval xTime ;	
	int xRet ; 
	int index ;
	xRet = gettimeofday(&xTime, NULL); 
	if(isMs == 1){
		index = xTime.tv_usec/1000 ; // unit: ms
	}else{
		index = xTime.tv_sec ; // unit: s
	}

	return index ; 
}

static u8 noSensitiveDataCnt = 0 ;
static u32 maxIndex = 1 ;
static u32 tmpCurTime = 0 ;
int Handler456(u8 * dataAddr , u8* isReport){
	u32 tmpCntL=0 , tmpCntR=0 , tmpCntC=0;

	getTargetAreaAverData( 0 , dataAddr , &tmpCntL) ;
	getTargetAreaAverData( 1 , dataAddr , &tmpCntR) ;
	getTargetAreaAverData( 2 , dataAddr , &tmpCntC) ;



	if((tmpCntL+tmpCntR)<ldwsCtrl.threshold){
		//debugl(" ---  SensitiveDataCnt  LR:[%d] - C:[%d]  --- " , tmpCntL+tmpCntR , tmpCntC ) ;
	}else{
		//debugl(" SensitiveDataCnt  LR:[%d] - C:[%d] " , tmpCntL+tmpCntR , tmpCntC ) ;
	}

	tmpCurTime = getSysTimeMs(0) ;
	
	if((tmpCntC > 120) && (tmpCntC <1000 ) &&((tmpCntL<20) && (tmpCntR<20)) &&(tmpCurTime>ldwsCtrl.curTime)){
	//if((tmpCntC > 120) && (tmpCntC <2000 ) &&((tmpCntL<30) && (tmpCntR<30)) ){		
		ldwsCtrl.curTime  = tmpCurTime+1 ;
		debugl(">>> ldws cnt:[%d] - [%d] - [%d]<<<" ,tmpCntL , tmpCntC , tmpCntR) ;

		*(u8*)isReport = 1 ;
		
	}else{

		//debugl(">>>cnt:[%d] - [%d] - [%d] -tmpMaxL[%d] - tmpMaxR[%d]<<<" ,tmpCntL , tmpCntC , tmpCntR , tmpMaxL , tmpMaxR) ;
		*(u8*)isReport = 0 ;
	}
	
#if 0
	//if(tmpCntL >= ldwsCtrl.threshold || tmpCntR >= ldwsCtrl.threshold){
	if((tmpCntL+tmpCntR) >= ldwsCtrl.threshold){
		ldwsCtrl.curTime = getSysTimeMs(0) ;
		if(ldwsCtrl.haveSensitiveData == 0){	
			ldwsCtrl.startTime = ldwsCtrl.curTime ;
		}
		ldwsCtrl.haveSensitiveData = 1 ;

		ldwsCtrl.duration = ldwsCtrl.curTime - ldwsCtrl.startTime ;

		noSensitiveDataCnt = 0 ;

		//debugl("startTime:[%d] - curTime[%d]" , ldwsCtrl.startTime , ldwsCtrl.curTime ) ;

	}else{

//		if(noSensitiveDataCnt >= 8 ){// 5 
		if(noSensitiveDataCnt >= 3 || ldwsCtrl.duration >= 10 || tmpCntC>0){// 5 
			//debugl("no detect lane ... cntL:[%d] - cntR:[%d]" , tmpCntL , tmpCntR) ;
			ldwsCtrl.haveSensitiveData = 0 ;
			noSensitiveDataCnt = 0 ;
		}else{
			noSensitiveDataCnt++ ;
		}
	}

	//if(ldwsCtrl.duration >= 6  && ldwsCtrl.haveSensitiveData == 0 ){ //12 	// time unit:s	
	if(ldwsCtrl.duration >= 4  && ldwsCtrl.haveSensitiveData == 0 ){ //4 - 20190516
		if( tmpCntC >= 12 ){		
				debugl(">>> LDWS - [%d][%d][%d]<<<" , tmpCntL , tmpCntC , tmpCntR) ;
			*(u8*)isReport = 1 ;
		//	debugl("duration[%d] - have[%d] - L:[%d] - R:[%d]" , ldwsCtrl.duration , \ 
		//	ldwsCtrl.haveSensitiveData , tmpCntL , tmpCntR ) ;
			ldwsCtrl.duration = 0 ;
		}else{
			;//debugl(">>>cnt:[%d] - [%d]s<<<" , tmpCntC , ldwsCtrl.duration) ;
		}

	}else{
		*(u8*)isReport = 0 ;
	}
#endif
	
	tmpSumL += tmpMaxL ;
	tmpSumR += tmpMaxR ;

	if(maxIndex>=40){ // 100
		u8 tmpL , tmpR ;
		u8 tmpVal = 0 ;

		tmpL = (tmpSumL/maxIndex) ;
	#if 0
		if(tmpL>200){
			tmpVal = tmpL - 55 ;
		}else{
			tmpVal = tmpL - 30 ;
		}
	#else
		if(tmpL > 240){
			tmpVal = (u8)tmpL*0.92 ;
		}else if(tmpL > 220){
			tmpVal = (u8)tmpL*0.88 ;
		}else if(tmpL > 200){
			tmpVal = (u8)tmpL*0.85 ;			
		}else if(tmpL > 160){
			tmpVal = (u8)tmpL*0.83 ;
		}else if(tmpL > 130){
			tmpVal = (u8)tmpL*0.97 ;
		}else{
			tmpVal = tmpL ;
		}
		
	#endif


		//if(tmpVal>100)
		{
			lanePosData.thresholdL = tmpVal ; // 35
		}


		tmpR = (tmpSumR/maxIndex) ;
	#if 0
		if(tmpR>200){
			tmpVal = tmpR - 45 ;
		}else{
			tmpVal = tmpR - 25 ;
		}
	#else
		if(tmpR > 240){
			tmpVal = (u8)tmpR*0.93 ;
		}else if(tmpR> 220){
			tmpVal = (u8)tmpR*0.90 ;
		}else if(tmpR > 200){
			tmpVal = (u8)tmpR*0.87 ;			
		}else if(tmpR > 160){
			tmpVal = (u8)tmpR*0.83 ;
		}else if(tmpR > 130){
			tmpVal = (u8)tmpR*0.98 ;
		}else{
			tmpVal = tmpR ;
		}

	#endif


		//if(tmpVal>90)
		{
			lanePosData.thresholdR = tmpVal ;
		}

	//	debugl("thresholdL:[%d][%d] - thresholdR:[%d][%d]" , tmpSumL , lanePosData.thresholdL , \
	//		tmpSumR , lanePosData.thresholdR) ;
		maxIndex = 0 ;
		tmpSumL = 0 ;
		tmpSumR = 0  ;
	}
	maxIndex++ ;
	
//	debugl("avrL:[%d] - avrR:[%d]" , tmpAvrL/tmpIndexL , tmpAvrR/tmpIndexR) ;
	tmpAvrL = 0 ;
	tmpAvrR = 0 ;
	tmpIndexL = 0 ;
	tmpIndexR = 0 ;
	
	return 0 ;
}


#endif

